/*
 * Scheduler.cpp
 *
 *      Author: Christian
 *      Letztes Update: 03.11.13 Christian | Code Aufräumen
 *
 */

#include "Scheduler.h"

volatile uint8_t status = 0;
volatile int16_t sensor_puffer[6] = { 0, 0, 0, 0, 0, 0 };

// Intervall Routine

ISR( TIMER0_COMP_vect ) {
	TIMSK &= ~(1 << OCIE0);
	// Interrupts wieder Einschalten um Empfang von Fernsteuerung nicht zu stoeren
	sei();
	int16_t temp = 0;


//	if ((status & 0xC0) == 0xC0) {
//		OCR0 = 157;
//		status &= ~(0xC0);
//	} else {
//		OCR0 = 156;
//		status += 0x40;
//	}

	mpu_read_value( GYRO_XOUT_H, &temp);
	sensor_puffer[0] = temp;
	mpu_read_value( GYRO_YOUT_H, &temp);
	sensor_puffer[1] = temp;
	mpu_read_value( GYRO_ZOUT_H, &temp);
	sensor_puffer[2] = temp;
	mpu_read_value( ACCEL_XOUT_H, &temp);
	sensor_puffer[3] = temp - xA_off;
	mpu_read_value( ACCEL_YOUT_H, &temp);
	sensor_puffer[4] = temp - yA_off;
	mpu_read_value( ACCEL_ZOUT_H, &temp);
	sensor_puffer[5] = temp - zA_off;

	status |= (1 << NEWSENS);
	TIMSK |= (1 << OCIE0);
}

void scheduler_start() {

	// Timer Output Compare einstellen; Prescaler: /1024
	TCCR0 |= (1 << WGM01) | (1 << CS02) | (1 << CS00);

	// Timer Output Compare Interrupt

	TIMSK |= (1 << OCIE0);

	// Timer Interrupt Compare wert			// bei 10 ms -> interrupt alle 9,984 ms
	//OCR0 =   F_CPU / 1024000 * ABTASTZEIT_IN_MS;

//	OCR0 = 156;
	OCR0 = 78;
}

void system_pause() {
	cli();

	// Scheduler beenden;
	TCCR0 = 0;

	// Empftimer beenden
	TCCR1B = 0;

	// Timer interrupts ausschalten
	TIMSK = 0;

	system_pause_signal();

}

void system_reset() {

	// Watchdog einschalten
	WDTCR = (1 << WDE);

	// Dauerschleife
	while (1)
		;

}
